// **********************
// PI 2008 - Symbios
// **********************

//**********************************************************
//************   EXTRACTION DE FOND (EDF) ******************
//**********************************************************

//includes
#include <cv.h>
#include <highgui.h>
#include <stdio.h>


//Data
IplImage *IavgF,*IdiffF, *IprevF, *IhiF, *IlowF;
IplImage *Iscratch,*Iscratch2;
//Float, 1-channel images
//
IplImage *Igray1,*Igray2, *Igray3;
IplImage *Ilow1, *Ilow2, *Ilow3;
IplImage *Ihi1,   *Ihi2, *Ihi3;
// Byte, 1-channel image
//
IplImage *Imaskt;
//Counts number of images learned for averaging later.
//
float Icount;



void DeallocateImages()
{
   cvReleaseImage( &IavgF);
   cvReleaseImage( &IdiffF );
   cvReleaseImage( &IprevF );
   cvReleaseImage( &IhiF );
   cvReleaseImage( &IlowF );
   cvReleaseImage( &Ilow1 );
   cvReleaseImage( &Ilow2 );
   cvReleaseImage( &Ilow3 );
   cvReleaseImage( &Ihi1 );
   cvReleaseImage( &Ihi2 );
   cvReleaseImage( &Ihi3 );
   cvReleaseImage( &Iscratch );
   cvReleaseImage( &Iscratch2 );
   cvReleaseImage( &Igray1 );
   cvReleaseImage( &Igray2 );
   cvReleaseImage( &Igray3 );
   cvReleaseImage( &Imaskt);
}



    // Create a binary: 0,255 mask where 255 means foreground pixel
    // I        Input image, 3-channel, 8u
    // Imask Mask image to be created, 1-channel 8u
    //
    void backgroundDiff( IplImage *I,IplImage *Imask) {
       cvCvtScale(I,Iscratch,1,0); // To float;
       cvSplit( Iscratch, Igray1,Igray2,Igray3, 0 );
       //Channel 1
       cvInRange(Igray1,Ilow1,Ihi1,Imask);
  //Channel 2
  //
  cvInRange(Igray2,Ilow2,Ihi2,Imaskt);
  cvOr(Imask,Imaskt,Imask,NULL);
  //Channel 3
  //
  cvInRange(Igray3,Ilow3,Ihi3,Imaskt);
  cvOr(Imask,Imaskt,Imask,NULL);
  //Finally, invert the results
  //
  cvSubRS( Imask, cvScalar(255,255,255,255), Imask,NULL);
}


void setHighThreshold( float scale )
{
   cvConvertScale( IdiffF, Iscratch, scale,0 );
   cvAdd( Iscratch, IavgF, IhiF ,NULL);
   cvSplit( IhiF, Ihi1, Ihi2, Ihi3, 0 );
}
void setLowThreshold( float scale )
{
   cvConvertScale( IdiffF, Iscratch, scale,0 );
   cvSub( IavgF, Iscratch, IlowF ,NULL);
   cvSplit( IlowF, Ilow1, Ilow2, Ilow3, 0 );
}


void createModelsfromStats() {
    cvConvertScale( IavgF, IavgF,( double)(1.0/Icount),0 );
    cvConvertScale( IdiffF, IdiffF,(double)(1.0/Icount),0 );
    //Make sure diff is always something
    cvAddS( IdiffF, cvScalar( 1.0, 1.0, 1.0,1.0), IdiffF ,NULL);
    setHighThreshold( 4.0 );
    setLowThreshold( 4.0 );
}


void accumulateBackground( IplImage *I ){
   static int first = 1;                   // nb. Not thread safe
   cvCvtScale( I, Iscratch, 1, 0 );      // convert to float
   if( !first ){
      cvAcc( Iscratch, IavgF ,NULL);
      cvAbsDiff( Iscratch, IprevF, Iscratch2 );
      cvAcc( Iscratch2, IdiffF ,NULL);
      Icount += 1.0;
   }
   first = 0;
   cvCopy( Iscratch, IprevF ,NULL);
}


// I is just a sample image for allocation purposes
// (passed in for sizing)
//
void AllocateImages( IplImage* I ){
   CvSize sz = cvGetSize( I );
   IavgF     = cvCreateImage( sz, IPL_DEPTH_32F,  3 );
   IdiffF    = cvCreateImage( sz, IPL_DEPTH_32F,  3 );
   IprevF    = cvCreateImage( sz, IPL_DEPTH_32F,  3 );
   IhiF      = cvCreateImage( sz, IPL_DEPTH_32F,  3 );
   IlowF     = cvCreateImage( sz, IPL_DEPTH_32F,  3 );
   Ilow1     = cvCreateImage( sz, IPL_DEPTH_32F,  1 );
   Ilow2     = cvCreateImage( sz, IPL_DEPTH_32F,  1 );
   Ilow3     = cvCreateImage( sz, IPL_DEPTH_32F,  1 );
   Ihi1      = cvCreateImage( sz, IPL_DEPTH_32F,  1 );
   Ihi2      = cvCreateImage( sz, IPL_DEPTH_32F,  1 );
   Ihi3      = cvCreateImage( sz, IPL_DEPTH_32F,  1 );
   cvZero( IavgF );
   cvZero( IdiffF );
   cvZero( IprevF );
   cvZero( IhiF );
   cvZero( IlowF );
   Icount    = 0.00001; //Protect against divide  by zero
   Iscratch = cvCreateImage(   sz, IPL_DEPTH_32F, 3 );
   Iscratch2 = cvCreateImage(  sz, IPL_DEPTH_32F, 3 );
   Igray1    = cvCreateImage(  sz, IPL_DEPTH_32F, 1 );
   Igray2    = cvCreateImage(  sz, IPL_DEPTH_32F, 1 );
   Igray3    = cvCreateImage(  sz, IPL_DEPTH_32F, 1 );
   Imaskt    = cvCreateImage(  sz, IPL_DEPTH_8U,  1 );
   cvZero( Iscratch );
   cvZero( Iscratch2 );
}


//EDF
int edf() {

//Capture CAM
CvCapture* capture = cvCaptureFromCAM( CV_CAP_ANY );
if( !capture ) {
    fprintf( stderr, "ERROR: capture is NULL \n" );
    getchar();
    return -1;
    }



cvNamedWindow( "mywindow", CV_WINDOW_AUTOSIZE );
cvNamedWindow( "mywindow2", CV_WINDOW_AUTOSIZE );
IplImage* fond = cvQueryFrame( capture );
AllocateImages(fond);



cvWaitKey(0);

//Apprentissage du fon
int n_modele=0;
for( n_modele=0; n_modele<40; n_modele++) {
	fond = cvQueryFrame( capture );
	if( !fond ) {
		fprintf( stderr, "ERROR: frame is null...\n" );
		getchar();
		break;
		}
     	cvShowImage( "mywindow", fond );
	accumulateBackground(fond);
     	if( (cvWaitKey(10) & 255) == 27 ) break;

	} 

createModelsfromStats();


IplImage* result =  cvCreateImage( cvGetSize(fond), 8 , 1 );
cvWaitKey(0);
//Soustraction de fond
while( 1 ) {
	//Recuperer l image courante
	IplImage* frame = cvQueryFrame( capture );
	if( !frame ) {
		fprintf( stderr, "ERROR: frame is null...\n" );
		getchar();
		break;
		}
	//Soustraction et seuillage	
	backgroundDiff( frame, result);
	//Affichage image
	cvShowImage( "mywindow", frame );
	cvShowImage( "mywindow2", result );
	if( (cvWaitKey(10) & 255) == 27 ) break;
	}

//Desalocation ressource
cvReleaseCapture( &capture );
cvDestroyWindow( "mywindow" );
cvDestroyWindow( "mywindow2" );
cvReleaseImage(&result);

return 1;
}